#pragma once

#include <iostream>
#include <fstream>
#include <string>

#include <Eigen/Core>

#include <opencv2/core.hpp>

void LoadExtrinsic(const std::string &filename, Eigen::Matrix4d &extrinsic) {
  cv::FileStorage fs;
  fs.open(filename, cv::FileStorage::READ);
  if (!fs.isOpened()) {
    std::cerr << "Failed to open " << filename << std::endl;
    return;
  }
  cv::FileNode extr = fs["roatation_matrix"];
  extrinsic << extr[0][0], extr[0][1], extr[0][2], extr[0][3], extr[1][0],
      extr[1][1], extr[1][2], extr[1][3], extr[2][0], extr[2][1], extr[2][2],
      extr[2][3], extr[3][0], extr[3][1], extr[3][2], extr[3][3];

  fs.release();
  return;
}

void LoadExtrinsic(const std::string &filename, Eigen::Matrix4d &extrinsic, 
        std::string &frame_id, std::string &child_id) {
  cv::FileStorage fs;
  fs.open(filename, cv::FileStorage::READ);
  if (!fs.isOpened()) {
    std::cerr << "Failed to open " << filename << std::endl;
    return;
  }
  cv::FileNode extr = fs["roatation_matrix"];
  extrinsic << extr[0][0], extr[0][1], extr[0][2], extr[0][3], extr[1][0],
      extr[1][1], extr[1][2], extr[1][3], extr[2][0], extr[2][1], extr[2][2],
      extr[2][3], extr[3][0], extr[3][1], extr[3][2], extr[3][3];
  frame_id = fs["main_sensor"].string();
  child_id = fs["child_sensor"].string();
  fs.release();
  return;
}

void LoadExtrinsic(const std::string &filename, Eigen::Matrix4d &extrinsic, 
        std::string &frame_id, std::string &child_id, 
        std::string &main_topic, std::string &child_topic) {
  cv::FileStorage fs;
  fs.open(filename, cv::FileStorage::READ);
  if (!fs.isOpened()) {
    std::cerr << "Failed to open " << filename << std::endl;
    return;
  }
  cv::FileNode extr = fs["roatation_matrix"];
  extrinsic << extr[0][0], extr[0][1], extr[0][2], extr[0][3], extr[1][0],
      extr[1][1], extr[1][2], extr[1][3], extr[2][0], extr[2][1], extr[2][2],
      extr[2][3], extr[3][0], extr[3][1], extr[3][2], extr[3][3];
  frame_id = fs["main_sensor"].string();
  child_id = fs["child_sensor"].string();
  main_topic = fs["main_topic"].string();
  child_topic = fs["child_topic"].string();
  fs.release();
  return;
}

void SaveExtrinsicYaml(const std::string &filename, const std::string &frame_id,
      const std::string &child_id, Eigen::Matrix4d &extrinsic) {

  std::ofstream fCalib(filename);
  if (!fCalib.is_open()) {
    std::cerr << "open file " << filename << " failed." << std::endl;
    return;
  }
  Eigen::Quaterniond q(extrinsic.block<3,3>(0,0));

  fCalib << "header:\n"
         << "  seq: 0\n"
         << "  stamp:\n"
         << "    secs: 0\n"
         << "    nsecs: 0\n"
         << "  frame_id: " << frame_id << "\n";
  fCalib << "child_frame_id: " << child_id  << std::endl;
  fCalib << "transform:" << std::endl;
  fCalib << "  rotation:\n"
         << "    x: " << q.x() << "\n"
         << "    y: " << q.y() << "\n"
         << "    z: " << q.z() << "\n"
         << "    w: " << q.w() << "\n"
         << "  translation:\n"
         << "    x: " << extrinsic(0, 3) << "\n"
         << "    y: " << extrinsic(1, 3) << "\n"
         << "    z: " << extrinsic(2, 3) << std::endl;

  fCalib.close();
}
